#include "perf_controller.h"


#define PLLC0_EMUCNT0  *((volatile uint32_t *)0x01c11F0)
#define PLLC0_EMUCNT1  *((volatile uint32_t *)0x01c11F4)

static bool pm_inited = FALSE;

void pm_init(void)
{
    // Initialize PLLC0 Emulation performance counter
    PLLC0_EMUCNT0 = 1;  // trigger counters
    pm_inited = TRUE;
    return;
}

err_t get_clock_cycles(cycles_t *ptr)
{
    if (!ptr) { return ERR_INVALID; }
    if (!pm_inited) { return ERR_NOT_INITIALIZED; }

    // @todo: possible race condition with a context
    // switch happening just before reading PLLC0_EMUCNT1
    ptr->cycles_lo = PLLC0_EMUCNT0;
    ptr->cycles_hi = PLLC0_EMUCNT1;

    return ERR_SUCCESS;
}

err_t clock_cycles_diff(cycles_t *a, cycles_t *b, cycles_t *diff)
{
    unsigned long long al;
    unsigned long long bl;

    if (!a || !b || !diff) { return ERR_INVALID; }

    al = ((unsigned long long)a->cycles_hi << 32) | a->cycles_lo;
    bl = ((unsigned long long)b->cycles_hi << 32) | b->cycles_lo;

    if (al >= bl) {
        al = al - bl;
    } else {
        al = bl - al;
    }

    diff->cycles_lo = (al & 0xFFFFFFFF);
    diff->cycles_hi = (al >> 32);

    return ERR_SUCCESS;
 }


